#ifndef _MAPPROCESSOR_H
#define _MAPPROCESSOR_H

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <lanelet2_io/Io.h>
#include <lanelet2_projection/UTM.h>
#include <lanelet2_traffic_rules/TrafficRules.h>
#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingCost.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_routing/RoutingGraphContainer.h>

#include "module_base/util.h"
#include "cuda_base/cuda_base.h"
#include "obstacle_detector/util.h"
#include "obstacle_detector/debug_display.h"
#include <iostream>

namespace obstacle_detector
{
class MapProcessor
{
public:
	MapProcessor(bool debug);
	~MapProcessor(){};
	void set_map(std::string map_path);
	void map_process(const module_base::Location &loc,
		int16_t *points, uint16_t *points_order,int &point_num,
		uint8_t *point_road_index,int &roads_num);
	void get_roads_yaw(std::vector<float> &roads_yaw);

private:
	void check_point_road(const module_base::Location &loc,
		uint8_t *point_road_index,int &roads_num,int point_num);
	void get_related_area(const module_base::Location &loc,
		const std::vector<Region> all_areas,std::vector<float> &area_data,
		std::vector<Eigen::MatrixXf> &vertexs, float threshold);
	void get_related_fence(const module_base::Location &loc,
		const std::vector<Region> all_areas,std::vector<float> &area_data,
		std::vector<Eigen::MatrixXf> &vertexs, float threshold);

private:
	lanelet::LaneletMapPtr map_;
	bool debug_;

	const float min_x_=-30;  //检测范围
	const float max_x_=80;
	const float min_y_=-50;
	const float max_y_=50;

	float *static_area_gpu_;
	float *road_area_gpu_;
	bool *valid_index_gpu_;
	int16_t *valid_points_gpu_;
	uint16_t *valid_points_order_gpu_;
	int *valid_points_num_gpu_;

	std::vector<float> roads_yaw_;

	std::vector<Region> all_static_areas_;
	std::vector<Region> all_fence_areas_;
	std::vector<Region> all_road_areas_;

	std::unique_ptr<DebugDisplay> debug_display_;



};//class
}

#endif